#include "costmap_2d.h"

namespace costmap_2d{

Costmap2D::Costmap2D(string name, bool is_local, tf2_ros::Buffer* tf_buffer)
    :name_(name),isLocal_(is_local),tfBuffer_(tf_buffer),originCostmapPtr_(nullptr),
    realtimeCostmapPtr_(nullptr),staticLayerPtr_(nullptr),inflationLayerPtr_(nullptr){
    realtimeCostmapPtr_ = boost::make_shared<Costmap>();
    // 代价地图参数初始化
    if(is_local){
        LocalParamsInit();
    }
    else{
        GlobalParamsInit();
    }
    // 图层数据
    staticLayerPtr_ = boost::make_shared<StaticLayer>(realtimeCostmapPtr_, tf_buffer);
    sensorLayerPtr_ = boost::make_shared<SensorLayer>(realtimeCostmapPtr_, tf_buffer);
    inflationLayerPtr_ = boost::make_shared<InflationLayer>(realtimeCostmapPtr_, 0.2);
}

Costmap2D::~Costmap2D(){

}

// 参数初始化
void Costmap2D::GlobalParamsInit(){
    realtimeCostmapPtr_->frameId = ros::param::param<std::string>("/global_costmap/frame_id", std::string("map"));
}

// 参数初始化
void Costmap2D::LocalParamsInit(){
    realtimeCostmapPtr_->frameId = ros::param::param<std::string>("/local_costmap/frame_id", std::string("base_link"));
    double windows = ros::param::param<double>("/local_costmap/windows", 4.0);
    auto& mapMetaData = realtimeCostmapPtr_->map_metadata;
    mapMetaData.resolution = ros::param::param<double>("/local_costmap/resolution", 0.02);
    mapMetaData.width = int(windows / mapMetaData.resolution);
    mapMetaData.height = int(windows / mapMetaData.resolution);
    mapMetaData.origin.position.x = -(windows / 2.0);
    mapMetaData.origin.position.y = -(windows / 2.0);
    realtimeCostmapPtr_->costMap.resize(mapMetaData.width*mapMetaData.height, costmap_2d::FREE_SPACE);
}

// 设置地图数据
void Costmap2D::setMapData(nav_msgs::OccupancyGrid::Ptr map_ptr){
    originCostmapPtr_ = boost::make_shared<Costmap>();
    originCostmapPtr_->setMap(map_ptr);
}

void Costmap2D::setLidar2DScanData(sensor_msgs::LaserScan::Ptr sensor_ptr){
    sensorLayerPtr_->addLidar2DScanData(sensor_ptr);
}

void Costmap2D::setPointCloud2Data(sensor_msgs::PointCloud2::Ptr sensor_ptr){
    sensorLayerPtr_->addPointCloud2Data(sensor_ptr);
}

// 获取代价地图
boost::shared_ptr<Costmap> Costmap2D::getCostMap(){
    return realtimeCostmapPtr_;
}

// 获取栅格地图
void Costmap2D::getOccupancyGrid(nav_msgs::OccupancyGrid& map_msg){
    if(!realtimeCostmapPtr_) return;
    map_msg.header.frame_id = realtimeCostmapPtr_->frameId;
    map_msg.header.stamp = ros::Time::now();
    map_msg.info = realtimeCostmapPtr_->map_metadata;
    // map_msg.info.resolution = realtimeCostmapPtr_->map_metadata.resolution;
    // map_msg.info.width = realtimeCostmapPtr_->map_metadata.width;
    // map_msg.info.height = realtimeCostmapPtr_->map_metadata.height;
    // map_msg.info.origin.position.x = realtimeCostmapPtr_->map_metadata.position.x;
    // map_msg.info.origin.position.y = realtimeCostmapPtr_->map_metadata.position.x;
    // map_msg.info.origin.orientation.w = 1.0;
    map_msg.data.resize(realtimeCostmapPtr_->costMap.size());
    for(size_t i=0; i<realtimeCostmapPtr_->costMap.size(); ++i){
        unsigned char value = char(realtimeCostmapPtr_->costMap[i]);
        if(value == costmap_2d::LETHAL_OBSTACLE){
            map_msg.data[i] = 100;
        }
        else if(value == costmap_2d::UNKNOWN){
            map_msg.data[i] = -1;
        }
        else if(value == costmap_2d::FREE_SPACE){
            map_msg.data[i] = 0;
        }
        else{
            double rate = double(value) / double(costmap_2d::LETHAL_OBSTACLE - 1);
            map_msg.data[i] = char((rate) * 100.0);
            // cout << "value: " << int(value) << ", cost " << int(map_msg.data[i])  << ", rate: " << rate << endl;
        }     
    }
}

// 更新代价地图
void Costmap2D::updateCostmap(){
    // 校验原始地图数据是否已经设置
    if(!originCostmapPtr_){
        LOG(WARNING) << "[Costmap2D::updateCostmap] originCostmapPtr_ is nullptr!" << endl;
        return;
    }
    if(!realtimeCostmapPtr_){
        LOG(WARNING) << "[Costmap2D::updateCostmap] realtimeCostmapPtr_ is nullptr!" << endl;
        return;
    }

    // 更新静态地图层 障碍物、虚拟墙、禁行区等数据
    if(!staticLayerPtr_->hasSetMap()){
        staticLayerPtr_->setMap(originCostmapPtr_);
    }

    staticLayerPtr_->updateCostmap(isLocal_);
    auto static_obstacle_cells = staticLayerPtr_->getObstacleCells();   //静态障碍物数据

    // 更新传感器地图层
    sensorLayerPtr_->updateCostmap(isLocal_);
    auto sensors_obstacle_cells = sensorLayerPtr_->getObstacleCells();   //传感器障碍物数据

    if(!isLocal_){
        cout << "sensors_obstacle_cells size: " << sensors_obstacle_cells.size() << endl;
    }

    // 更新膨胀地图层
    std::unordered_map<int, GridCell> obstacle_cells;
    obstacle_cells.insert(static_obstacle_cells.begin(), static_obstacle_cells.end());
    obstacle_cells.insert(sensors_obstacle_cells.begin(), sensors_obstacle_cells.end());
    inflationLayerPtr_->updateCostmap(obstacle_cells);
}

// 清除代价地图
void Costmap2D::clearCostmap(){
    originCostmapPtr_ = nullptr;
    realtimeCostmapPtr_ = nullptr;
    staticLayerPtr_->clearCostmap();
    sensorLayerPtr_->clearCostmap();
    inflationLayerPtr_->clearCostmap();
}

}; //costmap_2d